


#include "isp.h"



#if (FFC_FKB_CC_STATIC == 1)
static char fkb_cache[FFC_FKB_CC_SIZE];
#endif



/**
 * @brief FFC FastKB calibration calculate
 * 
 * @param[in] lframes    : low temp frames address
 * @param[in] hframes    : high temp frames address
 * @param[in] num_frames : frames number
 * @param[in] dim        : frame dim
 * @param[in] box        : roi box
 * @param[in] expaddr    : calculate result export address
 * 
 * @return qe_ret
 * 
 * This function will perform two steps:
 *   1. Traverse the high and low temp frames to obtain the mean 
 *      value 'lt_mean/ht_mean' and mean frame
 *   2. Calculate G/O parameters from 'lt_mean/ht_mean' and average frame
 * 
 * @note
 * High and low temperature calibration frames are stored in the DDR. In 
 * order to reduce memory access and improve computing efficiency, BRAM is 
 * used to cache the intermediate process of computing.
 */
qe_ret ffc_fastkb(void *lframes, void *hframes, unsigned int num_frames, 
    qe_dim2 dim, qe_box box, void *expaddr)
{
    int xl, xh;
    float vl, vh, tmp;
    char *cache_buffer;
    qe_u32 g, o;
    qe_u32 *go;
    qe_u32 ts, t1, t2;
    qe_u32 cache_size;
    qe_u32 pix_per_cache;
    qe_u32 bytes_per_cache;
    qe_u32 pix_per_box;
    qe_u32 pix_per_frame;
    qe_u32 *lfs;
    qe_u32 *hfs;
    qe_u32 *lcs, *hcs;
    qe_u32 *lcache, *hcache;
    qe_u32 *lcache_temp, *hcache_temp;
    qe_u64 lsum, hsum;
    

    /**
     * lfs  : pointer to the low temp frames start
     * hfs  : pointer to the high temp frames start
     * lcs  : pointer to cache location in low temp frames
     * hcs  : pointer to cache location in high temp frames
     */

    if (!lframes || !hframes || !num_frames || 
        !dim.x || !dim.y || !box.w || !box.h)
        return qe_err_param;

    if (dim.y % FFC_FKB_CC_LINES) {
        isp_error("dim not match");
        return qe_err_param;
    }

    /*
     * Step1: Calculate mean value and mean frame
     */
    lsum               = 0;
    hsum               = 0;
    lfs                = (qe_u32 *)lframes;
    hfs                = (qe_u32 *)hframes;
    go                 = (qe_u32 *)expaddr;
    pix_per_frame      = dim.x * dim.y;
    pix_per_cache      = dim.x * FFC_FKB_CC_LINES;
    pix_per_box        = box.w * box.h;
    bytes_per_cache    = sizeof(qe_u32) * pix_per_cache;
    cache_size         = bytes_per_cache * 4;

#if (CONFIG_FKB_DEBUG == 1)
    /*for (int i=0; i<pix_per_frame; i++) {
        go[i] = i;
    }*/
    for (int i=0; i<dim.x; i++) {
        for (int j=0; j<dim.y; j++) {
            if ((i < 320) && (j < 256)) {
                go[j*dim.x + i] = 0;
            } else if ((i >= 320) && (j < 256)) {
                go[j*dim.x + i] = 4096;
            } else if ((i < 320) && (j >= 256)) {
                go[j*dim.x + i] = 8192;
            } else if ((i >= 320) && (j >= 256)) {
                go[j*dim.x + i] = 16383;
            }
        }
    }
    isp_info("fastkb debug output");
    return qe_ok;
#endif

#if (FFC_FKB_CC_STATIC == 1)
    cache_buffer = fkb_cache;
#else
    cache_buffer = qe_malloc(cache_size);
    if (!cache_buffer) {
        ffc_error("fastkb alloc err");
        return qe_err_mem;
    }
#endif
   qe_memset(cache_buffer, 0x0, cache_size);
    lcache      = (qe_u32 *)cache_buffer;
    hcache      = lcache + pix_per_cache;
    lcache_temp = lcache + pix_per_cache * 2;
    hcache_temp = lcache + pix_per_cache * 3;

    isp_debug("pix per frame %d", pix_per_frame);
    isp_debug("pix per cache %d", pix_per_cache);
    isp_debug("bytes per cache %d", bytes_per_cache);
    isp_debug("dim {%d,%d} box (%d,%d,%d,%d)", dim.x, dim.y, box.x, box.y, box.w, box.h);

    ts = qe_time_ms();
    t1 = ts;
    isp_info("fastkb step1 start");

    for (int j=box.y; j<(box.h+box.y); j+=FFC_FKB_CC_LINES) {

        /* clean l/h cache */
        qe_memset(lcache, 0x0, bytes_per_cache * 2);

        for (int i=0; i<num_frames; i++) {

            /* copy rows to cache */
            lcs = &lfs[pix_per_frame * i + dim.x * j];
            hcs = &hfs[pix_per_frame * i + dim.x * j];
            //isp_debug("lcs 0x%x hcs 0x%x", lcs, hcs);
            qe_memcpy(lcache_temp, lcs, bytes_per_cache);
            qe_memcpy(hcache_temp, hcs, bytes_per_cache);

            /* cache sum */
            for (int n=0; n<FFC_FKB_CC_LINES; n++) {
                for (int k=box.x; k<(box.x+box.w); k++) {
                    //isp_debug("pix %d", dim.x * n + k);
                    lcache[dim.x * n + k] += lcache_temp[dim.x * n + k];
                    hcache[dim.x * n + k] += hcache_temp[dim.x * n + k];
                    lsum += lcache_temp[dim.x * n + k];
                    hsum += hcache_temp[dim.x * n + k];
                }
            }

            /* write back cache to first frame */
            qe_memcpy(&lfs[dim.x * j], lcache, bytes_per_cache);
            qe_memcpy(&hfs[dim.x * j], hcache, bytes_per_cache);
        }
    }

    /* sum to mean */
    for (int j=box.y; j<(box.y+box.h); j++) {
        for (int i=box.x; i<(box.x+box.w); i++) {
            lfs[dim.x * j + i] /= num_frames;
            hfs[dim.x * j + i] /= num_frames;
        }
    }

    vl = (float)lsum / (pix_per_box * num_frames);
    vh = (float)hsum / (pix_per_box * num_frames);
    isp_info("fastkb vl %d vh %d", (qe_u32)vl, (qe_u32)vh);

    t2 = qe_time_ms();
    isp_info("fastkb step1 fin in %dms", t2-t1);


    /*
     * Step2: 
     */
    t1 = qe_time_ms();
    isp_info("fastkb step2 start");

    for (int j=box.y; j<(box.y+box.h); j++) {
        
        for (int i=box.x; i<(box.x+box.w); i++) {

            xl = (int)lfs[dim.x * j + i];
            xh = (int)hfs[dim.x * j + i];
            tmp = (vh - vl) / (xh - xl);
            if (tmp < 0.5 || tmp > 1.5) {
                g = 1;
                o = 0;
            } else {
                g = tmp * 8192;
                tmp = (vh * xl - vl * xh) / (xl - xh);
                if (tmp < 0) {
                    tmp = -tmp + 0.5;
                    o   = tmp;
                    o  |= 0x8000;
                } else {
                    o   = tmp + 0.5;
                }
            }
            go[dim.x * j + i] = o | (g << 16);
        }
    }

    t2 = qe_time_ms();
    isp_info("fastkb step2 fin in %dms", t2-t1);
    isp_info("fastkb runtime %dms", t2-ts);

#if (FFC_FKB_CC_STATIC == 0)
    qe_free(cache_buffer);
#endif

    return qe_ok;
}